
//#define DEBUG
//#define PROFILING
//#define VISUALIZE

using UnityEngine;
using System.Diagnostics;
using System.Collections;
using System.Collections.Generic;

//[System.Serializable] // For debugging only
public class LegState
{

    public Vector3 stepFromPosition;
    public Vector3 stepToPosition;
    public Vector3 stepToPositionGoal;
    public Matrix4x4 stepFromMatrix;
    public Matrix4x4 stepToMatrix;
    public float stepFromTime;
    public float stepToTime;

    public int stepNr = 0;

    public float cycleTime = 1;
    public float designatedCycleTimePrev = 0.9f;
    public Vector3 hipReference;
    public Vector3 ankleReference;
    public Vector3 footBase;
    public Quaternion footBaseRotation;
    public Vector3 ankle;

    public float stanceTime = 0;
    public float liftTime = 0.1f;
    public float liftoffTime = 0.2f;
    public float postliftTime = 0.3f;
    public float prelandTime = 0.7f;
    public float strikeTime = 0.8f;
    public float landTime = 0.9f;

    public bool parked;

    public Vector3 stancePosition;
    public Vector3 heelToetipVector;

    public List<string> debugHistory = new List<string>();

    public float GetFootGrounding(float time)
    {
        if ((time <= liftTime) || (time >= landTime)) return 0;
        if ((time >= postliftTime) && (time <= prelandTime)) return 1;
        if (time < postliftTime)
        {
            return (time - liftTime) / (postliftTime - liftTime);
        }
        else
        {
            return 1 - (time - prelandTime) / (landTime - prelandTime);
        }
    }
}

public class MotionGroupState
{
    public AnimationState controller;
    public float weight;
    public AnimationState[] motionStates;
    public float[] relativeWeights;
    public int primaryMotionIndex;
}

[RequireComponent(typeof(LegController))]
[RequireComponent(typeof(AlignmentTracker))]
public class LegAnimator : MonoBehaviour
{

    public bool startAutomatically = true;

    public float minStepDistance = 0.2f;
    public float maxStepDuration = 1.5f;
    public float maxStepRotation = 160;
    public float maxStepAcceleration = 5.0f;
    public float maxStepHeight = 1.0f;
    public float maxSlopeAngle = 60;
    public bool enableLegParking = true;
    public LayerMask groundLayers = 1;

    public float groundHugX = 0;
    public float groundHugZ = 0;
    public float climbTiltAmount = 0.5f;
    public float climbTiltSensitivity = 0.0f;
    public float accelerateTiltAmount = 0.02f;
    public float accelerateTiltSensitivity = 0.0f;

    // Debug settings
    public bool renderFootMarkers = false;
    public bool renderBlendingGraph = false;
    public bool renderCycleGraph = false;
    public bool renderAnimationStates = false;

    private bool isActive;
    private float currentTime;

    private LegController legC;
    private AlignmentTracker tr;
    private LegInfo[] legs;
    private LegState[] legStates;

    private Vector3 position;
    private Vector3 horizontalVelocity;
    private Vector3 horizontalVelocitySmoothed;
    private float speed;
    private float speedSmoothed;
    private Vector3 objectVelocity;
    private Vector3 usedObjectVelocity;
    private Quaternion rotation;
    private Vector3 up;
    private Vector3 forward;
    private float scale;
    private Vector3 baseUpGround;
    private Vector3 bodyUp;
    private Vector3 legsUp;
    private float accelerationTiltX;
    private float accelerationTiltZ;

    private AnimationState controlMotionState;
    private MotionGroupState[] motionGroupStates;
    private AnimationState[] nonGroupMotionStates;
    private float[] nonGroupMotionWeights;

    private AnimationState[] motionStates;
    private AnimationState[] cycleMotionStates;
    private float[] motionWeights;
    private float[] cycleMotionWeights;
    private float summedMotionWeight;
    private float summedCycleMotionWeight;
    private float locomotionWeight;

    private float cycleDuration;
    private float cycleDistance;
    private float normalizedTime;

    private bool updateStates = true;

    [System.NonSerialized]
    public GameObject ghost;

    private Dictionary<string, TrajectoryVisualizer> trajectories
        = new Dictionary<string, TrajectoryVisualizer>();

    [Conditional("VISUALIZE")]
    void AddTrajectoryPoint(string name, Vector3 point)
    {
        trajectories[name].AddPoint(Time.time, point);
    }

    [Conditional("DEBUG")]
    void Assert(bool condition, string text)
    {
        if (!condition) UnityEngine.Debug.LogError(text);
    }

    [Conditional("DEBUG")]
    void AssertSane(float f, string text)
    {
        if (!Util.IsSaneNumber(f)) UnityEngine.Debug.LogError(text + "=" + f);
    }

    [Conditional("DEBUG")]
    void AssertSane(Vector3 vect, string text)
    {
        if (!Util.IsSaneNumber(vect.x)
            || !Util.IsSaneNumber(vect.y)
            || !Util.IsSaneNumber(vect.z)
        ) UnityEngine.Debug.LogError(text + "=" + vect);
    }

    [Conditional("DEBUG")]
    void AssertSane(Quaternion q, string text)
    {
        if (!Util.IsSaneNumber(q.x)
            || !Util.IsSaneNumber(q.y)
            || !Util.IsSaneNumber(q.z)
            || !Util.IsSaneNumber(q.w)
        ) UnityEngine.Debug.LogError(text + "=" + q);
    }

    void Start()
    {
        tr = GetComponent(typeof(AlignmentTracker)) as AlignmentTracker;
        legC = GetComponent(typeof(LegController)) as LegController;
        legs = legC.legs;
        if (!legC.initialized)
        {
            UnityEngine.Debug.LogError(name + ": Locomotion System has not been initialized.", this);
            enabled = false;
        }

        legStates = new LegState[legs.Length];

        updateStates = true;
        ResetMotionStates();

        isActive = false;

        for (int leg = 0; leg < legs.Length; leg++)
        {
            trajectories.Add(
                "leg" + leg + "heel",
                new TrajectoryVisualizer(legs[leg].debugColor, 3)
            );
            trajectories.Add(
                "leg" + leg + "toetip",
                new TrajectoryVisualizer(legs[leg].debugColor, 3)
            );
            trajectories.Add(
                "leg" + leg + "footbase",
                new TrajectoryVisualizer(legs[leg].debugColor, 3)
            );
        }
    }

    void OnEnable()
    {
        updateStates = true;
        if (legC == null) return;
        ResetMotionStates();
        if (!legC.initialized)
        {
            UnityEngine.Debug.LogError(name + ": Locomotion System has not been initialized.", this);
            enabled = false;
        }
    }

    private AnimationClip GetDummyClip()
    {
        AnimationClip clip = new AnimationClip();
        AnimationCurve curve = new AnimationCurve(new Keyframe(0, 0), new Keyframe(1, 1));
        clip.SetCurve("dummy_path", typeof(Transform), "dummy_property", curve);
        return clip;
    }

    private void ResetMotionStates()
    {
        motionStates = new AnimationState[legC.motions.Length];
        cycleMotionStates = new AnimationState[legC.cycleMotions.Length];
        motionWeights = new float[legC.motions.Length];
        cycleMotionWeights = new float[legC.cycleMotions.Length];
        nonGroupMotionWeights = new float[legC.nonGroupMotions.Length];

        controlMotionState = animation["LocomotionSystem"];
        if (controlMotionState == null)
        {
            animation.AddClip(GetDummyClip(), "LocomotionSystem");
            controlMotionState = animation["LocomotionSystem"];
        }
        controlMotionState.enabled = true;
        controlMotionState.wrapMode = WrapMode.Loop;
        controlMotionState.weight = 1;
        controlMotionState.layer = 10000;

        motionGroupStates = new MotionGroupState[legC.motionGroups.Length];
        int cm = 0;
        for (int m = 0; m < legC.motions.Length; m++)
        {
            motionStates[m] = animation[legC.motions[m].name];
            if (motionStates[m] == null)
            {
                animation.AddClip(legC.motions[m].animation, legC.motions[m].name);
                motionStates[m] = animation[legC.motions[m].name];
            }
            motionStates[m].wrapMode = WrapMode.Loop;
            if (legC.motions[m].motionType == MotionType.WalkCycle)
            {
                cycleMotionStates[cm] = motionStates[m];
                cycleMotionStates[cm].speed = 0;
                cm++;
            }
        }

        for (int g = 0; g < motionGroupStates.Length; g++)
        {
            AnimationState controller = animation[legC.motionGroups[g].name];
            if (controller == null)
            {
                animation.AddClip(GetDummyClip(), legC.motionGroups[g].name);
                controller = animation[legC.motionGroups[g].name];
            }
            controller.enabled = true;
            controller.wrapMode = WrapMode.Loop;
            if (startAutomatically && g == 0) controller.weight = 1;

            motionGroupStates[g] = new MotionGroupState();
            motionGroupStates[g].controller = controller;
            motionGroupStates[g].motionStates = new AnimationState[legC.motionGroups[g].motions.Length];
            motionGroupStates[g].relativeWeights = new float[legC.motionGroups[g].motions.Length];
            for (int m = 0; m < motionGroupStates[g].motionStates.Length; m++)
            {
                motionGroupStates[g].motionStates[m] = animation[legC.motionGroups[g].motions[m].name];
                motionGroupStates[g].relativeWeights[m] = 0;
            }
            motionGroupStates[g].primaryMotionIndex = 0;
        }

        nonGroupMotionStates = new AnimationState[legC.nonGroupMotions.Length];
        for (int m = 0; m < legC.nonGroupMotions.Length; m++)
        {
            nonGroupMotionStates[m] = animation[legC.nonGroupMotions[m].name];
            if (nonGroupMotionStates[m] == null)
            {
                animation.AddClip(legC.nonGroupMotions[m].animation, legC.nonGroupMotions[m].name);
                nonGroupMotionStates[m] = animation[legC.nonGroupMotions[m].name];
                nonGroupMotionWeights[m] = nonGroupMotionStates[m].weight;
            }
        }

        for (int leg = 0; leg < legs.Length; leg++)
        {
            legStates[leg] = new LegState();
        }
    }

    private void ResetSteps()
    {
        up = transform.up;
        forward = transform.forward;
        baseUpGround = up;
        legsUp = up;
        bodyUp = up;

        for (int leg = 0; leg < legs.Length; leg++)
        {
            legStates[leg].stepFromTime = Time.time - 0.01f;
            legStates[leg].stepToTime = Time.time;

            legStates[leg].stepFromMatrix = FindGroundedBase(
                transform.TransformPoint(legStates[leg].stancePosition / scale),
                transform.rotation,
                legStates[leg].heelToetipVector,
                false
            );
            legStates[leg].stepFromPosition = legStates[leg].stepFromMatrix.GetColumn(3);

            legStates[leg].stepToPosition = legStates[leg].stepFromPosition;
            legStates[leg].stepToMatrix = legStates[leg].stepFromMatrix;
        }
        normalizedTime = 0;

        cycleDuration = maxStepDuration;
        cycleDistance = 0;
    }

    void Update()
    {

        if (Time.deltaTime == 0) return;

#if (PROFILING)
		JCsProfilerMethod pm1;
#endif

        scale = transform.lossyScale.z;

        AssertSane(tr.velocity, "tr.velocity");
        AssertSane(up, "up");
        horizontalVelocity = Util.ProjectOntoPlane(tr.velocity, up);
        horizontalVelocitySmoothed = Util.ProjectOntoPlane(tr.velocitySmoothed, up);
        speed = horizontalVelocity.magnitude;
        speedSmoothed = horizontalVelocitySmoothed.magnitude;

        objectVelocity = (
            transform.InverseTransformPoint(horizontalVelocitySmoothed)
            - transform.InverseTransformPoint(Vector3.zero)
        );

#if (PROFILING)
		pm1 = JCsProfiler.Instance.StartCallStopWatch(
			"ProfilingExample", gameObject.name, "Update Weights");
#endif

        bool newVelocity = false;
        if (
            (objectVelocity - usedObjectVelocity).magnitude
            >
            0.002f * Mathf.Min(objectVelocity.magnitude, usedObjectVelocity.magnitude)
            ||
            updateStates
        )
        {
            newVelocity = true;
            usedObjectVelocity = objectVelocity;
        }

        bool newWeights = false;
        float smallWeightDifference = 0.01f * 0.1f;

        for (int g = 0; g < legC.motionGroups.Length; g++)
        {
            MotionGroupState group = motionGroupStates[g];

            bool changedGroupWeight = false;
            bool justEnabled = false;
            float newGroupWeight = group.controller.weight;
            if (group.controller.enabled == false || newGroupWeight < smallWeightDifference) newGroupWeight = 0;
            else if (newGroupWeight > 1 - smallWeightDifference) newGroupWeight = 1;
            if (Mathf.Abs(newGroupWeight - group.weight) > smallWeightDifference)
            {
                changedGroupWeight = true;
                newWeights = true;
                if (group.weight == 0 && newGroupWeight > 0) justEnabled = true;
                group.weight = newGroupWeight;
            }
            else if (
                Mathf.Abs(
                    group.motionStates[group.primaryMotionIndex].weight
                    - group.relativeWeights[group.primaryMotionIndex] * group.weight
                ) > smallWeightDifference
                ||
                group.motionStates[group.primaryMotionIndex].layer
                != group.controller.layer
            )
            {
                changedGroupWeight = true;
            }

            if (newVelocity || changedGroupWeight)
            {

                if ((newVelocity || justEnabled) && group.weight > 0)
                {
                    newWeights = true;

#if (PROFILING)
					pm1.CallIsFinished();
					pm1 = JCsProfiler.Instance.StartCallStopWatch(
						"ProfilingExample", gameObject.name, "Interpolate");
#endif

                    MotionGroupInfo groupInfo = legC.motionGroups[g];
                    group.relativeWeights = groupInfo.GetMotionWeights(objectVelocity);

#if (PROFILING)
					pm1.CallIsFinished();
					pm1 = JCsProfiler.Instance.StartCallStopWatch(
						"ProfilingExample", gameObject.name, "Update Weights");
#endif
                }

                if (group.weight > 0)
                {
                    float highestWeight = 0;
                    int controllerLayer = group.controller.layer;
                    for (int m = 0; m < group.motionStates.Length; m++)
                    {
                        float weight = group.relativeWeights[m] * group.weight;
                        group.motionStates[m].weight = weight;
                        if (weight > 0) group.motionStates[m].enabled = true;
                        else group.motionStates[m].enabled = false;
                        group.motionStates[m].layer = controllerLayer;

                        if (weight > highestWeight)
                        {
                            group.primaryMotionIndex = m;
                            highestWeight = weight;
                        }
                    }
                }
                else
                {
                    for (int m = 0; m < group.motionStates.Length; m++)
                    {
                        group.motionStates[m].weight = 0;
                        group.motionStates[m].enabled = false;
                    }
                }
            }
        }

        for (int m = 0; m < nonGroupMotionStates.Length; m++)
        {
            float newWeight = nonGroupMotionStates[m].weight;
            if (nonGroupMotionStates[m].enabled == false) newWeight = 0;
            if (
                Mathf.Abs(newWeight - nonGroupMotionWeights[m]) > smallWeightDifference
                || (newWeight == 0 && nonGroupMotionWeights[m] != 0)
            )
            {
                newWeights = true;
                nonGroupMotionWeights[m] = newWeight;
            }
        }

#if (PROFILING)
		pm1.CallIsFinished();
#endif

        bool justActivated = updateStates;
        if (newWeights || updateStates)
        {
#if (PROFILING)
			pm1 = JCsProfiler.Instance.StartCallStopWatch(
				"ProfilingExample", gameObject.name, "Apply Weights");
#endif

            summedMotionWeight = 0;
            summedCycleMotionWeight = 0;
            int cm = 0;
            for (int m = 0; m < legC.motions.Length; m++)
            {
                motionWeights[m] = motionStates[m].weight;
                summedMotionWeight += motionWeights[m];
                if (legC.motions[m].motionType == MotionType.WalkCycle)
                {
                    cycleMotionWeights[cm] = motionWeights[m];
                    summedCycleMotionWeight += motionWeights[m];
                    cm++;
                }
            }
            if (summedMotionWeight == 0)
            {
                isActive = false;
#if (PROFILING)
				pm1.CallIsFinished();
#endif
                if (ghost != null)
                {
                    GhostOriginal go = ghost.GetComponent(typeof(GhostOriginal)) as GhostOriginal;
                    go.Synch();
                }
                return;
            }
            else
            {
                if (isActive == false) justActivated = true;
                isActive = true;
            }

            for (int m = 0; m < legC.motions.Length; m++)
            {
                motionWeights[m] /= summedMotionWeight;
            }
            if (summedCycleMotionWeight > 0)
            {
                for (int m = 0; m < legC.cycleMotions.Length; m++)
                {
                    cycleMotionWeights[m] /= summedCycleMotionWeight;
                }
            }

            for (int leg = 0; leg < legs.Length; leg++)
            {
                legStates[leg].stancePosition = Vector3.zero;
                legStates[leg].heelToetipVector = Vector3.zero;
            }
            for (int m = 0; m < legC.motions.Length; m++)
            {
                IMotionAnalyzer motion = legC.motions[m];
                float weight = motionWeights[m];
                if (weight > 0)
                {
                    for (int leg = 0; leg < legs.Length; leg++)
                    {
                        legStates[leg].stancePosition += (
                            motion.cycles[leg].stancePosition * scale * weight
                        );
                        legStates[leg].heelToetipVector += (
                            motion.cycles[leg].heelToetipVector * scale * weight
                        );
                    }
                }
            }

            if (summedCycleMotionWeight > 0)
            {
                for (int leg = 0; leg < legs.Length; leg++)
                {
                    legStates[leg].liftTime = 0;
                    legStates[leg].liftoffTime = 0;
                    legStates[leg].postliftTime = 0;
                    legStates[leg].prelandTime = 0;
                    legStates[leg].strikeTime = 0;
                    legStates[leg].landTime = 0;
                }
                for (int m = 0; m < legC.cycleMotions.Length; m++)
                {
                    IMotionAnalyzer motion = legC.cycleMotions[m];
                    float weight = cycleMotionWeights[m];
                    if (weight > 0)
                    {
                        for (int leg = 0; leg < legs.Length; leg++)
                        {
                            legStates[leg].liftTime += motion.cycles[leg].liftTime * weight;
                            legStates[leg].liftoffTime += motion.cycles[leg].liftoffTime * weight;
                            legStates[leg].postliftTime += motion.cycles[leg].postliftTime * weight;
                            legStates[leg].prelandTime += motion.cycles[leg].prelandTime * weight;
                            legStates[leg].strikeTime += motion.cycles[leg].strikeTime * weight;
                            legStates[leg].landTime += motion.cycles[leg].landTime * weight;
                        }
                    }
                }
            }

            if (summedCycleMotionWeight > 0)
            {
                for (int leg = 0; leg < legs.Length; leg++)
                {
                    Vector2 stanceTimeVector = Vector2.zero;
                    for (int m = 0; m < legC.cycleMotions.Length; m++)
                    {
                        IMotionAnalyzer motion = legC.cycleMotions[m];
                        float weight = cycleMotionWeights[m];
                        if (weight > 0)
                        {
                            stanceTimeVector += new Vector2(
                                Mathf.Cos(motion.cycles[leg].stanceTime * 2 * Mathf.PI),
                                Mathf.Sin(motion.cycles[leg].stanceTime * 2 * Mathf.PI)
                            ) * weight;
                        }
                    }
                    legStates[leg].stanceTime = Util.Mod(
                        Mathf.Atan2(stanceTimeVector.y, stanceTimeVector.x) / 2 / Mathf.PI
                    );
                }
            }

#if (PROFILING)
			pm1.CallIsFinished();
#endif
        }

        float controlMotionStateWeight = controlMotionState.weight;
        if (!controlMotionState.enabled) controlMotionStateWeight = 0;
        locomotionWeight = Mathf.Clamp01(summedMotionWeight * controlMotionStateWeight);
        if (updateStates || justActivated) ResetSteps();

#if (PROFILING)
		pm1 = JCsProfiler.Instance.StartCallStopWatch(
			"ProfilingExample", gameObject.name, "Cycle Calc");
#endif


        float cycleFrequency = 0;
        float animatedCycleSpeed = 0;
        for (int m = 0; m < legC.motions.Length; m++)
        {
            IMotionAnalyzer motion = legC.motions[m];
            float weight = motionWeights[m];
            if (weight > 0)
            {
                if (motion.motionType == MotionType.WalkCycle)
                {
                    cycleFrequency += (1 / motion.cycleDuration) * weight;
                }
                animatedCycleSpeed += motion.cycleSpeed * weight;
            }
        }
        float desiredCycleDuration = maxStepDuration;
        if (cycleFrequency > 0) desiredCycleDuration = 1 / cycleFrequency;

        float speedMultiplier = 1;
        if (speed != 0) speedMultiplier = animatedCycleSpeed * scale / speed;
        if (speedMultiplier > 0) desiredCycleDuration *= Mathf.Sqrt(speedMultiplier);

        float verticalAngularVelocity = Vector3.Project(tr.rotation * tr.angularVelocitySmoothed, up).magnitude;
        if (verticalAngularVelocity > 0)
        {
            desiredCycleDuration = Mathf.Min(
                maxStepRotation / verticalAngularVelocity,
                desiredCycleDuration
            );
        }

        float groundAccelerationMagnitude = Util.ProjectOntoPlane(tr.accelerationSmoothed, up).magnitude;
        if (groundAccelerationMagnitude > 0)
        {
            desiredCycleDuration = Mathf.Clamp(
                maxStepAcceleration / groundAccelerationMagnitude,
                desiredCycleDuration / 2,
                desiredCycleDuration
            );
        }

        desiredCycleDuration = Mathf.Min(desiredCycleDuration, maxStepDuration);

        cycleDuration = desiredCycleDuration;

        AssertSane(cycleDuration, "cycleDuration");
        AssertSane(speed, "speed");
        cycleDistance = cycleDuration * speed;

        bool allParked = false;
        if (enableLegParking)
        {
            allParked = true;
            for (int leg = 0; leg < legs.Length; leg++)
            {
                if (legStates[leg].parked == false) allParked = false;
            }
        }

#if (PROFILING)
		pm1.CallIsFinished();
		pm1 = JCsProfiler.Instance.StartCallStopWatch(
			"ProfilingExample", gameObject.name, "Anim Synch");
#endif

        if (!allParked)
        {
            normalizedTime = Util.Mod(normalizedTime + (1 / cycleDuration) * Time.deltaTime);
            for (int m = 0; m < legC.cycleMotions.Length; m++)
            {
                if (legC.cycleMotions[m].GetType() == typeof(MotionAnalyzerBackwards))
                {
                    cycleMotionStates[m].normalizedTime = (
                        1 - (normalizedTime - legC.cycleMotions[m].cycleOffset)
                    );
                }
                else
                {
                    cycleMotionStates[m].normalizedTime = normalizedTime - legC.cycleMotions[m].cycleOffset;
                }
            }
        }

        updateStates = false;

        currentTime = Time.time;

#if (PROFILING)
		pm1.CallIsFinished();
#endif

        if (ghost != null)
        {
            GhostOriginal go = ghost.GetComponent(typeof(GhostOriginal)) as GhostOriginal;
            go.Synch();
        }
    }

    void FixedUpdate()
    {
        if (Time.deltaTime == 0) return;
        tr.ControlledFixedUpdate();
    }

    void LateUpdate()
    {
        if (Time.deltaTime == 0) return;

#if (PROFILING)
		JCsProfilerMethod pm1;
		pm1 = JCsProfiler.Instance.StartCallStopWatch(
			"ProfilingExample", gameObject.name, "Initial");
#endif

        tr.ControlledLateUpdate();
        position = tr.position;
        rotation = tr.rotation;

        AssertSane(tr.accelerationSmoothed, "acceleration");

        up = rotation * Vector3.up;
        forward = rotation * Vector3.forward;
        Vector3 right = rotation * Vector3.right;

#if (PROFILING)
		pm1.CallIsFinished();
#endif

        if (!isActive) return;
        if (currentTime != Time.time) return;

#if (PROFILING)
		pm1 = JCsProfiler.Instance.StartCallStopWatch(
			"ProfilingExample", gameObject.name, "Steps Loop");
#endif

        int origLayer = gameObject.layer;
        gameObject.layer = 2;

        for (int leg = 0; leg < legs.Length; leg++)
        {
            float designatedCycleTime = Util.CyclicDiff(normalizedTime, legStates[leg].stanceTime);

            bool newStep = false;
            if (designatedCycleTime < legStates[leg].designatedCycleTimePrev - 0.5f)
            {
                newStep = true;
                legStates[leg].stepNr++;
                if (!legStates[leg].parked)
                {
                    legStates[leg].stepFromTime = legStates[leg].stepToTime;
                    legStates[leg].stepFromPosition = legStates[leg].stepToPosition;
                    legStates[leg].stepFromMatrix = legStates[leg].stepToMatrix;
                    legStates[leg].debugHistory.Clear();
                    legStates[leg].cycleTime = designatedCycleTime;
                }
                legStates[leg].parked = false;

            }
            legStates[leg].designatedCycleTimePrev = designatedCycleTime;

            legStates[leg].stepToTime = (Time.time + (1 - designatedCycleTime) * cycleDuration);

            float predictedStrikeTime = (legStates[leg].strikeTime - designatedCycleTime) * cycleDuration;

            if ((designatedCycleTime >= legStates[leg].strikeTime))
                legStates[leg].cycleTime = designatedCycleTime;
            else
            {
                legStates[leg].cycleTime += ((legStates[leg].strikeTime - legStates[leg].cycleTime) * Time.deltaTime / predictedStrikeTime);
            }
            if ((legStates[leg].cycleTime >= designatedCycleTime))
                legStates[leg].cycleTime = designatedCycleTime;

            if (legStates[leg].cycleTime < legStates[leg].strikeTime)
            {
                float flightTime = Mathf.InverseLerp(legStates[leg].liftoffTime, legStates[leg].strikeTime, legStates[leg].cycleTime);

                Quaternion newPredictedRotation = Quaternion.AngleAxis(
                    tr.angularVelocitySmoothed.magnitude * (legStates[leg].stepToTime - Time.time),
                    tr.angularVelocitySmoothed) * tr.rotation;

                Quaternion predictedRotation;
                if (legStates[leg].cycleTime <= legStates[leg].liftoffTime)
                {
                    predictedRotation = newPredictedRotation;
                }
                else
                {
                    Quaternion oldPredictedRotation = Util.QuaternionFromMatrix(legStates[leg].stepToMatrix);
                    oldPredictedRotation =
                        Quaternion.FromToRotation(oldPredictedRotation * Vector3.up, up)
                        * oldPredictedRotation;

                    float rotationSeekSpeed = Mathf.Max(tr.angularVelocitySmoothed.magnitude * 3, maxStepRotation / maxStepDuration);
                    float maxRotateAngle = rotationSeekSpeed / flightTime * Time.deltaTime;
                    predictedRotation = Util.ConstantSlerp(oldPredictedRotation, newPredictedRotation, maxRotateAngle);
                }

                Vector3 newStepPosition;

                float turnSpeed = Vector3.Dot(tr.angularVelocitySmoothed, up);

                if (turnSpeed * cycleDuration < 5)
                {
                    newStepPosition = (tr.position + predictedRotation * legStates[leg].stancePosition
                        + horizontalVelocity * (legStates[leg].stepToTime - Time.time));
                }
                else
                {
                    Vector3 turnCenter = Vector3.Cross(up, horizontalVelocity) / (turnSpeed * Mathf.PI / 180);
                    Vector3 predPos = turnCenter + Quaternion.AngleAxis(turnSpeed * (legStates[leg].stepToTime - Time.time), up) * -turnCenter;

                    newStepPosition = (tr.position + predictedRotation * legStates[leg].stancePosition + predPos);
                }

                newStepPosition = Util.SetHeight(newStepPosition, position + legC.groundPlaneHeight * up * scale, up);

                Matrix4x4 groundedBase = FindGroundedBase(newStepPosition, predictedRotation, legStates[leg].heelToetipVector, true);
                newStepPosition = groundedBase.GetColumn(3);

                if (newStep)
                {
                    legStates[leg].stepToPosition = newStepPosition;
                    legStates[leg].stepToPositionGoal = newStepPosition;
                }
                else
                {
                    float stepSeekSpeed = Mathf.Max(speed * 3 + tr.accelerationSmoothed.magnitude / 10, legs[leg].footLength * scale * 3);

                    float towardStrike = legStates[leg].cycleTime / legStates[leg].strikeTime;

                    if ((newStepPosition - legStates[leg].stepToPosition).sqrMagnitude < Mathf.Pow(stepSeekSpeed * ((1 / towardStrike) - 1), 2))
                    {
                        legStates[leg].stepToPositionGoal = newStepPosition;
                    }

                    Vector3 moveVector = legStates[leg].stepToPositionGoal - legStates[leg].stepToPosition;
                    if (moveVector != Vector3.zero && predictedStrikeTime > 0)
                    {
                        float moveVectorMag = moveVector.magnitude;
                        float moveDist = Mathf.Min(moveVectorMag, Mathf.Max(stepSeekSpeed / Mathf.Max(0.1f, flightTime) * Time.deltaTime,
                                (1 + 2 * Mathf.Pow(towardStrike - 1, 2)) * (Time.deltaTime / predictedStrikeTime) * moveVectorMag));
                        legStates[leg].stepToPosition += ((legStates[leg].stepToPositionGoal - legStates[leg].stepToPosition)
                                                        / moveVectorMag * moveDist);
                    }
                }

                groundedBase.SetColumn(3, legStates[leg].stepToPosition);
                groundedBase[3, 3] = 1;
                legStates[leg].stepToMatrix = groundedBase;
            }

            if (enableLegParking)
            {
                float distToNextStep = Util.ProjectOntoPlane(legStates[leg].stepToPosition - legStates[leg].stepFromPosition, up).magnitude;

                bool significantStepDifference = (distToNextStep > minStepDistance || Vector3.Angle(legStates[leg].stepToMatrix.GetColumn(2),
                        legStates[leg].stepFromMatrix.GetColumn(2)) > maxStepRotation / 2);

                if (newStep && !significantStepDifference)
                {
                    legStates[leg].parked = true;
                }

                if (legStates[leg].parked && (designatedCycleTime < 0.67f) && significantStepDifference)
                {
                    legStates[leg].parked = false;
                }

                if (legStates[leg].parked) legStates[leg].cycleTime = 0;
            }
        }

#if (PROFILING)
		pm1.CallIsFinished();
		pm1 = JCsProfiler.Instance.StartCallStopWatch(
			"ProfilingExample", gameObject.name, "Base Calc");
#endif

        Vector3 tangentDir = Quaternion.Inverse(tr.rotation) * horizontalVelocity;
        tangentDir.y = 0;
        if (tangentDir.sqrMagnitude > 0) tangentDir = tangentDir.normalized;

        AssertSane(cycleDistance, "cycleDistance");

        Vector3[] basePointFoot = new Vector3[legs.Length];
        Vector3 basePoint = Vector3.zero;
        Vector3 baseVel = Vector3.zero;
        Vector3 avgFootPoint = Vector3.zero;
        float baseSummedWeight = 0.0f;
        for (int leg = 0; leg < legs.Length; leg++)
        {
            float weight = Mathf.Cos(legStates[leg].cycleTime * 2 * Mathf.PI) / 2.0f + 0.5f;
            baseSummedWeight += weight + 0.001f;

            float strideTime = Mathf.InverseLerp(legStates[leg].liftTime, legStates[leg].landTime, legStates[leg].cycleTime);
            float strideSCurve = -Mathf.Cos(strideTime * Mathf.PI) / 2f + 0.5f;

            Vector3 stepBodyPoint = transform.TransformDirection(-legStates[leg].stancePosition) * scale;

            AssertSane(legStates[leg].cycleTime, "legStates[leg].cycleTime");
            AssertSane(strideSCurve, "strideSCurve");
            AssertSane(tangentDir, "tangentDir");
            AssertSane(cycleDistance, "cycleDistance");
            AssertSane(legStates[leg].stepFromPosition, "legStates[leg].stepFromPosition");
            AssertSane(legStates[leg].stepToPosition, "legStates[leg].stepToPosition");
            AssertSane(legStates[leg].stepToMatrix.MultiplyVector(tangentDir), "stepToMatrix");
            AssertSane(legStates[leg].stepFromMatrix.MultiplyVector(tangentDir), "stepToMatrix");
            basePointFoot[leg] = ((legStates[leg].stepFromPosition + legStates[leg].stepFromMatrix.MultiplyVector(tangentDir)
                                    * cycleDistance * legStates[leg].cycleTime) * (1 - strideSCurve) + (legStates[leg].stepToPosition
                                    + legStates[leg].stepToMatrix.MultiplyVector(tangentDir) * cycleDistance * (legStates[leg].cycleTime - 1)
                                    ) * strideSCurve);
            AssertSane(basePointFoot[leg], "basePointFoot[leg]");
            if (System.Single.IsNaN(basePointFoot[leg].x) || System.Single.IsNaN(basePointFoot[leg].y) || System.Single.IsNaN(basePointFoot[leg].z))
            {
                UnityEngine.Debug.LogError("legStates[leg].cycleTime=" + legStates[leg].cycleTime + ", strideSCurve=" + strideSCurve + ", tangentDir=" + tangentDir + ", cycleDistance=" + cycleDistance + ", legStates[leg].stepFromPosition=" + legStates[leg].stepFromPosition + ", legStates[leg].stepToPosition=" + legStates[leg].stepToPosition + ", legStates[leg].stepToMatrix.MultiplyVector(tangentDir)=" + legStates[leg].stepToMatrix.MultiplyVector(tangentDir) + ", legStates[leg].stepFromMatrix.MultiplyVector(tangentDir)=" + legStates[leg].stepFromMatrix.MultiplyVector(tangentDir));
            }

            basePoint += (basePointFoot[leg] + stepBodyPoint) * (weight + 0.001f);
            avgFootPoint += basePointFoot[leg];

            baseVel += (legStates[leg].stepToPosition - legStates[leg].stepFromPosition) * (1f - weight + 0.001f);
        }
        Assert(baseSummedWeight != 0, "baseSummedWeight is zero");
        avgFootPoint /= legs.Length;
        basePoint /= baseSummedWeight;
        if (System.Single.IsNaN(basePoint.x) || System.Single.IsNaN(basePoint.y) || System.Single.IsNaN(basePoint.z))
            basePoint = position;
        AssertSane(basePoint, "basePoint");
        Vector3 groundBasePoint = basePoint + up * legC.groundPlaneHeight;

        Vector3 baseUp = up;
        if (groundHugX >= 0 || groundHugZ >= 0)
        {
            Vector3 baseUpGroundNew = up * 0.1f;
            for (int leg = 0; leg < legs.Length; leg++)
            {
                Vector3 vec = (basePointFoot[leg] - avgFootPoint);
                baseUpGroundNew += Vector3.Cross(Vector3.Cross(vec, baseUpGround), vec);
                UnityEngine.Debug.DrawLine(basePointFoot[leg], avgFootPoint);
            }

            float baseUpGroundNewUpPart = Vector3.Dot(baseUpGroundNew, up);
            if (baseUpGroundNewUpPart > 0)
            {
                baseUpGroundNew /= baseUpGroundNewUpPart;
                AssertSane(baseUpGroundNew, "baseUpGroundNew");
                baseUpGround = baseUpGroundNew;
            }

            if (groundHugX >= 1 && groundHugZ >= 1)
            {
                baseUp = baseUpGround.normalized;
            }
            else
            {
                baseUp = (up + groundHugX * Vector3.Project(baseUpGround, right) + groundHugZ * Vector3.Project(baseUpGround, forward)).normalized;
            }
        }

        Vector3 baseUpVel = up;
        if (baseVel != Vector3.zero) baseUpVel = Vector3.Cross(baseVel, Vector3.Cross(up, baseVel));
        baseUpVel /= Vector3.Dot(baseUpVel, up);

        Vector3 accelerationDir = Vector3.zero;
        if (accelerateTiltAmount * accelerateTiltSensitivity != 0)
        {
            float accelX = Vector3.Dot(tr.accelerationSmoothed * accelerateTiltSensitivity * accelerateTiltAmount, right) * (1 - groundHugX);
            float accelZ = Vector3.Dot(tr.accelerationSmoothed * accelerateTiltSensitivity * accelerateTiltAmount, forward) * (1 - groundHugZ);
            accelerationTiltX = Mathf.Lerp(accelerationTiltX, accelX, Time.deltaTime * 10);
            accelerationTiltZ = Mathf.Lerp(accelerationTiltZ, accelZ, Time.deltaTime * 10);
            accelerationDir = ((accelerationTiltX * right + accelerationTiltZ * forward) * (1 - 1 / (speedSmoothed * accelerateTiltSensitivity + 1)));
        }

        Vector3 tiltDir = Vector3.zero;
        if (climbTiltAmount * climbTiltAmount != 0)
        {
            tiltDir = ((Vector3.Project(baseUpVel, right) * (1 - groundHugX) + Vector3.Project(baseUpVel, forward) * (1 - groundHugZ))
                        * -climbTiltAmount * (1 - 1 / (speedSmoothed * climbTiltSensitivity + 1)));
        }

        bodyUp = (baseUp + accelerationDir + tiltDir).normalized;
        Quaternion bodyRotation = Quaternion.AngleAxis(Vector3.Angle(up, bodyUp), Vector3.Cross(up, bodyUp));

        legsUp = (up + accelerationDir).normalized;
        Quaternion legsRotation = Quaternion.AngleAxis(Vector3.Angle(up, legsUp), Vector3.Cross(up, legsUp));

#if (PROFILING)
		pm1.CallIsFinished();
#endif

        for (int leg = 0; leg < legs.Length; leg++)
        {
#if (PROFILING)
			pm1 = JCsProfiler.Instance.StartCallStopWatch(
				"ProfilingExample", gameObject.name, "Flight Path 1");
#endif

            float flightTime = Mathf.InverseLerp(legStates[leg].liftoffTime, legStates[leg].strikeTime, legStates[leg].cycleTime);

            float strideTime = Mathf.InverseLerp(legStates[leg].liftTime, legStates[leg].landTime, legStates[leg].cycleTime);

            int phase;
            float phaseTime = 0;
            if (legStates[leg].cycleTime < legStates[leg].liftoffTime)
            {
                phase = 0; phaseTime = Mathf.InverseLerp(0, legStates[leg].liftoffTime, legStates[leg].cycleTime);
            }
            else if (legStates[leg].cycleTime > legStates[leg].strikeTime)
            {
                phase = 2; phaseTime = Mathf.InverseLerp(legStates[leg].strikeTime, 1, legStates[leg].cycleTime);
            }
            else
            {
                phase = 1; phaseTime = flightTime;
            }

            Vector3 flightPos = Vector3.zero;
            for (int m = 0; m < legC.motions.Length; m++)
            {
                IMotionAnalyzer motion = legC.motions[m];
                float weight = motionWeights[m];
                if (weight > 0)
                {
                    flightPos += motion.GetFlightFootPosition(leg, phaseTime, phase) * weight;
                }
            }

            AssertSane(flightPos, "flightPos");

#if (PROFILING)
			pm1.CallIsFinished();
			pm1 = JCsProfiler.Instance.StartCallStopWatch(
				"ProfilingExample", gameObject.name, "Flight Path 2");
#endif

            Vector3 pointFrom = legStates[leg].stepFromPosition;
            Vector3 pointTo = legStates[leg].stepToPosition;
            Vector3 normalFrom = legStates[leg].stepFromMatrix.MultiplyVector(Vector3.up);
            Vector3 normalTo = legStates[leg].stepToMatrix.MultiplyVector(Vector3.up);
            Assert(Vector3.Dot(normalFrom, legsUp) > 0, "normalFrom and legsUp are perpendicular");
            Assert(Vector3.Dot(normalFrom, legsUp) > 0, "normalTo and legsUp are perpendicular");

            AssertSane(groundBasePoint, "groundBasePoint");
            AssertSane(baseUp, "baseUp");

            float flightProgressionLift = Mathf.Sin(flightPos.z * Mathf.PI);
            float flightTimeLift = Mathf.Sin(flightTime * Mathf.PI);

            legStates[leg].footBase = pointFrom * (1 - flightPos.z) + pointTo * flightPos.z;

            Vector3 offset = tr.position + tr.rotation * legStates[leg].stancePosition - Vector3.Lerp(pointFrom, pointTo, legStates[leg].cycleTime);

            legStates[leg].footBase += Util.ProjectOntoPlane(offset * flightProgressionLift, legsUp);

            AddTrajectoryPoint("leg" + leg + "footbase", legStates[leg].footBase + up * 0.01f + tr.rotation * legStates[leg].heelToetipVector * 0);

            AssertSane(legStates[leg].footBase, "legStates[leg].footBase");

            Vector3 midPoint = (pointFrom + pointTo) / 2;
            float tangentHeightFrom = (Vector3.Dot(normalFrom, pointFrom - midPoint) / Vector3.Dot(normalFrom, legsUp));
            float tangentHeightTo = (Vector3.Dot(normalTo, pointTo - midPoint) / Vector3.Dot(normalTo, legsUp));
            float heightMidOffset = Mathf.Max(tangentHeightFrom, tangentHeightTo) * 2 / Mathf.PI;
            AssertSane(heightMidOffset, "heightMidOffset");

            legStates[leg].footBase += heightMidOffset * flightProgressionLift * legsUp;
            AssertSane(legStates[leg].footBase, "legStates[leg].footBase");

#if (PROFILING)
			pm1.CallIsFinished();
			pm1 = JCsProfiler.Instance.StartCallStopWatch(
				"ProfilingExample", gameObject.name, "Flight Path 3");
#endif

            Quaternion footBaseRotationFromSteps = Quaternion.Slerp(Util.QuaternionFromMatrix(legStates[leg].stepFromMatrix),
                                                    Util.QuaternionFromMatrix(legStates[leg].stepToMatrix), flightTime);

            if (strideTime < 0.5)
            {
                legStates[leg].footBaseRotation = Quaternion.Slerp(Util.QuaternionFromMatrix(legStates[leg].stepFromMatrix), rotation, strideTime * 2);
            }
            else
            {
                legStates[leg].footBaseRotation = Quaternion.Slerp(rotation, Util.QuaternionFromMatrix(legStates[leg].stepToMatrix), strideTime * 2 - 1);
            }

            legStates[leg].footBaseRotation = Quaternion.FromToRotation(legStates[leg].footBaseRotation * Vector3.up, footBaseRotationFromSteps
                                                * Vector3.up) * legStates[leg].footBaseRotation;

            Vector3 footBaseElevated = Vector3.Lerp(legStates[leg].footBase, Util.SetHeight(legStates[leg].footBase, groundBasePoint, legsUp), flightTimeLift);

            if (Vector3.Dot(footBaseElevated, legsUp) > Vector3.Dot(legStates[leg].footBase, legsUp))
            {
                legStates[leg].footBase = footBaseElevated;
            }

            legStates[leg].footBase += flightPos.y * legsUp * scale;
            AssertSane(legStates[leg].footBase, "legStates[leg].footBase");

            Vector3 stepRight = Vector3.Cross(legsUp, pointTo - pointFrom).normalized;
            legStates[leg].footBase += flightPos.x * stepRight * scale;

            UnityEngine.Debug.DrawLine(legStates[leg].footBase, legStates[leg].footBase + legStates[leg].footBaseRotation
                                        * legStates[leg].heelToetipVector, legs[leg].debugColor);

#if (PROFILING)
			pm1.CallIsFinished();
#endif
        }

        if (locomotionWeight < 1)
        {
            for (int leg = 0; leg < legs.Length; leg++)
            {
                Vector3 footBaseReference = (-MotionAnalyzer.GetHeelOffset(legs[leg].ankle, legs[leg].ankleHeelVector, legs[leg].toe,
                                                legs[leg].toeToetipVector, legStates[leg].heelToetipVector, legStates[leg].footBaseRotation)
                                                + legs[leg].ankle.TransformPoint(legs[leg].ankleHeelVector));
                AssertSane(footBaseReference, "footBaseReference");
                legStates[leg].footBase = Vector3.Lerp(footBaseReference, legStates[leg].footBase, locomotionWeight);
                legStates[leg].footBaseRotation = Quaternion.Slerp(rotation, legStates[leg].footBaseRotation, locomotionWeight);
            }
        }

        legC.root.transform.rotation = (tr.rotation * Quaternion.Inverse(transform.rotation) * bodyRotation * legC.root.transform.rotation);
        for (int leg = 0; leg < legs.Length; leg++)
        {
            legs[leg].hip.rotation = legsRotation * Quaternion.Inverse(bodyRotation) * legs[leg].hip.rotation;
        }

        Vector3 rootPoint = legC.root.transform.position;
        Vector3 hipAverage = transform.TransformPoint(legC.hipAverage);
        Vector3 hipAverageGround = transform.TransformPoint(legC.hipAverageGround);
        Vector3 rootPointAdjusted = rootPoint;
        rootPointAdjusted += bodyRotation * (rootPoint - hipAverage) - (rootPoint - hipAverage);
        rootPointAdjusted += legsRotation * (hipAverage - hipAverageGround) - (hipAverage - hipAverageGround);
        legC.root.transform.position = rootPointAdjusted + position - transform.position;

        for (int leg = 0; leg < legs.Length; leg++)
        {
            legStates[leg].hipReference = legs[leg].hip.position;
            legStates[leg].ankleReference = legs[leg].ankle.position;
        }

        for (int pass = 1; pass <= 2; pass++)
        {
#if (PROFILING)
			pm1 = JCsProfiler.Instance.StartCallStopWatch(
				"ProfilingExample", gameObject.name, "Ankle Finding");
#endif

            for (int leg = 0; leg < legs.Length; leg++)
            {
                legStates[leg].ankle = MotionAnalyzer.GetAnklePosition(legs[leg].ankle, legs[leg].ankleHeelVector,
                                                legs[leg].toe, legs[leg].toeToetipVector, legStates[leg].heelToetipVector,
                                                legStates[leg].footBase, legStates[leg].footBaseRotation);
            }

#if (PROFILING)
			pm1.CallIsFinished();
			pm1 = JCsProfiler.Instance.StartCallStopWatch(
				"ProfilingExample", gameObject.name, "Hip Offset");
#endif

            FindHipOffset();

#if (PROFILING)
			pm1.CallIsFinished();
			pm1 = JCsProfiler.Instance.StartCallStopWatch(
				"ProfilingExample", gameObject.name, "Leg Adjusting");
#endif

            for (int leg = 0; leg < legs.Length; leg++) { AdjustLeg(leg, legStates[leg].ankle, pass == 2); }

#if (PROFILING)
			pm1.CallIsFinished();
#endif
        }

        for (int leg = 0; leg < legs.Length; leg++)
        {
            for (int i = 0; i < legs[leg].legChain.Length - 1; i++)
            {
                UnityEngine.Debug.DrawLine(legs[leg].legChain[i].position, legs[leg].legChain[i + 1].position, legs[leg].debugColor);
            }
        }

        Vector3 temp = position;
        UnityEngine.Debug.DrawRay(temp, up / 10, Color.white);
        UnityEngine.Debug.DrawRay(temp - forward / 20, forward / 10, Color.white);
        UnityEngine.Debug.DrawLine(hipAverage, hipAverageGround, Color.white);

        UnityEngine.Debug.DrawRay(temp, baseUp * 2, Color.blue);
        UnityEngine.Debug.DrawRay(hipAverage, bodyUp * 2, Color.yellow);

        gameObject.layer = origLayer;
    }

    public Matrix4x4 FindGroundedBase(Vector3 pos, Quaternion rot, Vector3 heelToToetipVector, bool avoidLedges)
    {
        RaycastHit hit;

        // Trace rays
        Vector3 hitAPoint = new Vector3();
        Vector3 hitBPoint = new Vector3();
        Vector3 hitANormal = new Vector3();
        Vector3 hitBNormal = new Vector3();
        bool hitA = false;
        bool hitB = false;
        bool valid = false;

        if (Physics.Raycast(pos + up * maxStepHeight, -up, out hit, maxStepHeight * 2, groundLayers))
        {
            valid = true;
            hitAPoint = hit.point;
            if (Vector3.Angle(hit.normal, up) < maxSlopeAngle)
            {
                hitANormal = hit.normal; hitA = true;
            }
        }

        Vector3 heelToToetip = rot * heelToToetipVector;
        float footLength = heelToToetip.magnitude;

        if (Physics.Raycast(pos + up * maxStepHeight + heelToToetip, -up, out hit, maxStepHeight * 2, groundLayers))
        {
            valid = true;
            hitBPoint = hit.point;
            if (Vector3.Angle(hit.normal, up) < maxSlopeAngle)
            {
                hitBNormal = hit.normal; hitB = true;
            }
        }

        if (!valid)
        {
            Matrix4x4 m = Matrix4x4.identity;
            m.SetTRS(pos, rot, Vector3.one);
            return m;
        }

        bool exclusive = false;
        if (avoidLedges)
        {
            if (!hitA && !hitB) hitA = true;
            else if (hitA && hitB)
            {
                Vector3 avgNormal = (hitANormal + hitBNormal).normalized;
                float hA = Vector3.Dot(hitAPoint, avgNormal);
                float hB = Vector3.Dot(hitBPoint, avgNormal);
                if (hA >= hB) hitB = false;
                else hitA = false;
                if (Mathf.Abs(hA - hB) > footLength / 4) exclusive = true;
            }
            else exclusive = true;
        }

        Vector3 newStepPosition;

        Vector3 stepUp = rot * Vector3.up;

        if (hitA)
        {
            if (hitANormal != Vector3.zero)
            {
                rot = Quaternion.FromToRotation(stepUp, hitANormal) * rot;
            }
            newStepPosition = hitAPoint;
            if (exclusive)
            {
                heelToToetip = rot * heelToToetipVector;
                newStepPosition -= heelToToetip * 0.5f;
            }
        }
        else
        {
            if (hitBNormal != Vector3.zero)
            {
                rot = Quaternion.FromToRotation(stepUp, hitBNormal) * rot;
            }
            heelToToetip = rot * heelToToetipVector;
            newStepPosition = hitBPoint - heelToToetip;
            if (exclusive) { newStepPosition += heelToToetip * 0.5f; }
        }

        return Util.MatrixFromQuaternionPosition(rot, newStepPosition);
    }

    public void FindHipOffset()
    {
        float lowestDesiredHeight = Mathf.Infinity;
        float lowestMaxHeight = Mathf.Infinity;
        float averageDesiredHeight = 0;
        AssertSane(legsUp, "legsUp");
        for (int leg = 0; leg < legs.Length; leg++)
        {
            float[] intersections;

            Vector3 desiredVector = (legStates[leg].ankleReference - legStates[leg].hipReference);
            float desiredDistance = desiredVector.magnitude;
            float desiredDistanceGround = Util.ProjectOntoPlane(desiredVector, legsUp).magnitude;

            Vector3 ankleVectorGround = Util.ProjectOntoPlane(legStates[leg].ankle - legs[leg].hip.position, legsUp);
            float excess = ankleVectorGround.magnitude - desiredDistanceGround;
            if (excess > 0)
            {
                float bufferDistance = (legs[leg].legLength * scale * 0.999f) - desiredDistanceGround;
                legStates[leg].ankle = (legStates[leg].ankle - ankleVectorGround + ankleVectorGround.normalized * (desiredDistanceGround
                                        + (1 - (1 / (excess / bufferDistance + 1))) * bufferDistance));
            }

            intersections = Util.GetLineSphereIntersections(Vector3.zero, legsUp, legStates[leg].ankle - legs[leg].hip.position, desiredDistance);
            float hipDesiredHeight;
            if (intersections != null)
            {
                hipDesiredHeight = intersections[1];
                AssertSane(hipDesiredHeight, "hipDesiredHeight (intersection)");
            }
            else
            {
                hipDesiredHeight = Vector3.Dot(legStates[leg].footBase - legs[leg].hip.position, legsUp);
            }

            intersections = Util.GetLineSphereIntersections(Vector3.zero, legsUp, legStates[leg].ankle - legs[leg].hip.position,
                                                            (legs[leg].legLength * scale * 0.999f));
            float hipMaxHeight;
            if (intersections != null) hipMaxHeight = intersections[1];
            else
            {
                hipMaxHeight = Vector3.Dot(legStates[leg].ankle - legs[leg].hip.position, legsUp);
                UnityEngine.Debug.Log(gameObject.name + ": Line-sphere intersection failed for leg " + leg + ", hipMaxHeight.");
            }

            if (hipDesiredHeight < lowestDesiredHeight) { lowestDesiredHeight = hipDesiredHeight; }
            if (hipMaxHeight < lowestMaxHeight) { lowestMaxHeight = hipMaxHeight; }
            averageDesiredHeight += hipDesiredHeight / legs.Length;
        }

        Assert(lowestDesiredHeight != Mathf.Infinity, "lowestDesiredHeight is infinite");
        Assert(lowestMaxHeight != Mathf.Infinity, "lowestMaxHeight is infinite");
        AssertSane(averageDesiredHeight, "averageDesiredHeight");
        AssertSane(lowestDesiredHeight, "lowestDesiredHeight");
        AssertSane(lowestMaxHeight, "lowestMaxHeight");

        if (lowestDesiredHeight > lowestMaxHeight) lowestDesiredHeight = lowestMaxHeight;
        float minToAvg = averageDesiredHeight - lowestDesiredHeight;
        float minToMax = lowestMaxHeight - lowestDesiredHeight;

        float hipHeight = lowestDesiredHeight;
        if (minToAvg + minToMax > 0)
        {
            hipHeight += minToAvg * minToMax / (minToAvg + minToMax);
        }

        AssertSane(hipHeight, "hipHeight");
        legC.root.position += hipHeight * legsUp;
    }

    public void AdjustLeg(int leg, Vector3 desiredAnklePosition, bool secondPass)
    {
        LegInfo legInfo = legs[leg];
        LegState legState = legStates[leg];

        Quaternion qAnkleOrigRotation;
        if (!secondPass)
        {
            Quaternion objectToFootBaseRotation = legStates[leg].footBaseRotation * Quaternion.Inverse(rotation);
            qAnkleOrigRotation = objectToFootBaseRotation * legInfo.ankle.rotation;
        }
        else
        {
            qAnkleOrigRotation = legInfo.ankle.rotation;
        }

        IKSolver ikSolver;
        if (legInfo.legChain.Length == 3) ikSolver = new IK1JointAnalytic();
        else ikSolver = new IKSimple();

        ikSolver.Solve(legInfo.legChain, desiredAnklePosition);

        Vector3 pHip = legInfo.hip.position;
        Vector3 pAnkle = legInfo.ankle.position;

        if (!secondPass)
        {
            Quaternion horizontalRotation = Quaternion.FromToRotation(forward, Util.ProjectOntoPlane(legStates[leg].footBaseRotation
                                                    * Vector3.forward, up)) * legInfo.ankle.rotation;

            legInfo.ankle.rotation = Quaternion.Slerp(horizontalRotation, qAnkleOrigRotation, 1 - legState.GetFootGrounding(legState.cycleTime));
        }
        else
        {
            Vector3 hipAnkleVector = pAnkle - pHip;
            Quaternion legAxisRotate = Quaternion.Slerp(Quaternion.identity, Quaternion.FromToRotation(Util.ProjectOntoPlane(forward, hipAnkleVector),
                                        Util.ProjectOntoPlane(legStates[leg].footBaseRotation * Vector3.forward, hipAnkleVector)), 0.5f);
            legInfo.hip.rotation = legAxisRotate * legInfo.hip.rotation;

            legInfo.ankle.rotation = qAnkleOrigRotation;
        }
    }

    void OnRenderObject()
    {
        CreateLineMaterial();
        lineMaterial.SetPass(0);

        if (!isActive) return;
        if (renderFootMarkers) RenderFootMarkers();
        if (renderBlendingGraph) RenderBlendingGraph();
        if (renderCycleGraph) RenderCycleGraph();
        foreach (KeyValuePair<string, TrajectoryVisualizer> kvp in trajectories)
        {
            kvp.Value.Render();
        }
    }

    [System.NonSerialized]
    private Material lineMaterial;

    private void CreateLineMaterial()
    {
        if (!lineMaterial)
        {
            lineMaterial = new Material("Shader \"Lines/Colored Blended\" {SubShader { Pass { BindChannels { Bind \"Color\",color }	Blend SrcAlpha OneMinusSrcAlpha	ZWrite Off Cull Off Fog { Mode Off } } } }");
            lineMaterial.hideFlags = HideFlags.HideAndDontSave;
            lineMaterial.shader.hideFlags = HideFlags.HideAndDontSave;
        }
    }

    private void DrawLine(Vector3 a, Vector3 b, Color color)
    {
        GL.Color(color);
        GL.Vertex(a);
        GL.Vertex(b);
    }

    public void RenderBlendingGraph()
    {
        Vector3 objectSpaceVelocity = transform.worldToLocalMatrix.MultiplyVector(horizontalVelocity);

        Matrix4x4 matrix = Util.CreateMatrix(transform.right, transform.forward, transform.up, transform.TransformPoint(legC.hipAverage));
        float size = (Camera.main.transform.position - transform.TransformPoint(legC.hipAverage)).magnitude / 2;
        DrawArea graph = new DrawArea3D(new Vector3(-size, -size, 0), new Vector3(size, size, 0), matrix);

        GL.Begin(GL.QUADS);
        graph.DrawRect(new Vector3(0, 0, 0), new Vector3(1, 1, 0), new Color(0, 0, 0, 0.2f));
        GL.End();

        Color weakColor = new Color(0.7f, 0.7f, 0.7f, 1);

        float range = 0;
        for (int i = 0; i < legC.motions.Length; i++)
        {
            IMotionAnalyzer m = legC.motions[i];
            range = Mathf.Max(range, Mathf.Abs(m.cycleVelocity.x));
            range = Mathf.Max(range, Mathf.Abs(m.cycleVelocity.z));
        }
        if (range == 0) range = 1;
        else range *= 1.2f;

        GL.Begin(GL.LINES);
        graph.DrawLine(new Vector3(0.5f, 0, 0), new Vector3(0.5f, 1, 0), weakColor);
        graph.DrawLine(new Vector3(0, 0.5f, 0), new Vector3(1, 0.5f, 0), weakColor);
        graph.DrawLine(new Vector3(0, 0, 0), new Vector3(1, 0, 0), weakColor);
        graph.DrawLine(new Vector3(1, 0, 0), new Vector3(1, 1, 0), weakColor);
        graph.DrawLine(new Vector3(1, 1, 0), new Vector3(0, 1, 0), weakColor);
        graph.DrawLine(new Vector3(0, 1, 0), new Vector3(0, 0, 0), weakColor);
        GL.End();

        float mX, mY;
        for (int g = 0; g < motionGroupStates.Length; g++)
        {
            Vector3 colorVect = Quaternion.AngleAxis((g + 0.5f) * 360.0f / motionGroupStates.Length, Vector3.one) * Vector3.right;
            Color color = new Color(colorVect.x, colorVect.y, colorVect.z);
            IMotionAnalyzer[] motions = legC.motionGroups[g].motions;

            GL.Begin(GL.QUADS);
            Color colorTemp = color * 0.4f;
            colorTemp.a = 0.8f;
            for (int i = 0; i < motions.Length; i++)
            {
                IMotionAnalyzer m = motions[i];
                mX = (m.cycleVelocity.x) / range / 2 + 0.5f;
                mY = (m.cycleVelocity.z) / range / 2 + 0.5f;
                float s = 0.02f;
                graph.DrawDiamond(new Vector3(mX - s, mY - s, 0), new Vector3(mX + s, mY + s, 0), colorTemp);
            }
            GL.End();
            if (motionGroupStates[g].weight == 0) continue;
            float[] weights = motionGroupStates[g].relativeWeights;
            GL.Begin(GL.QUADS);
            color.a = motionGroupStates[g].weight;
            for (int i = 0; i < motions.Length; i++)
            {
                IMotionAnalyzer m = motions[i];
                mX = (m.cycleVelocity.x) / range / 2 + 0.5f;
                mY = (m.cycleVelocity.z) / range / 2 + 0.5f;
                float s = Mathf.Pow(weights[i], 0.5f) * 0.05f;
                graph.DrawRect(new Vector3(mX - s, mY - s, 0), new Vector3(mX + s, mY + s, 0), color);
            }
            GL.End();
        }
        GL.Begin(GL.QUADS);
        mX = (objectSpaceVelocity.x) / range / 2 + 0.5f;
        mY = (objectSpaceVelocity.z) / range / 2 + 0.5f;
        float t = 0.02f;
        graph.DrawRect(new Vector3(mX - t, mY - t, 0), new Vector3(mX + t, mY + t, 0), new Color(0, 0, 0, 1));
        t /= 2;
        graph.DrawRect(new Vector3(mX - t, mY - t, 0), new Vector3(mX + t, mY + t, 0), new Color(1, 1, 1, 1));
        GL.End();
    }

    public void RenderCycleGraph()
    {
        float w = Camera.main.pixelWidth;
        float h = Camera.main.pixelHeight;
        Color weakColor = new Color(0.7f, 0.7f, 0.7f, 1);
        DrawArea graph = new DrawArea(new Vector3(w - 0.49f * h, 0.01f * h, 0), new Vector3(w - 0.01f * h, 0.49f * h, 0));
        graph.canvasMin = new Vector3(-1.1f, -1.1f, 0);
        graph.canvasMax = new Vector3(1.1f, 1.1f, 0);

        GL.Begin(GL.QUADS);
        graph.DrawRect(new Vector3(-1, -1, 0), new Vector3(1, 1, 0), new Color(0, 0, 0, 0.2f));
        GL.End();

        GL.Begin(GL.LINES);
        graph.DrawLine(-0.9f * Vector3.up, -1.1f * Vector3.up, weakColor);
        for (int i = 0; i < 90; i++)
        {
            graph.DrawLine(Quaternion.AngleAxis(i / 90.0f * 360, Vector3.forward) * Vector3.up, Quaternion.AngleAxis((i + 1) / 90.0f * 360,
                            Vector3.forward) * Vector3.up, weakColor);
        }
        for (int leg = 0; leg < legs.Length; leg++)
        {
            Color c = legs[leg].debugColor;
            Vector3 marker;
            marker = Quaternion.AngleAxis(360.0f * legStates[leg].liftoffTime, -Vector3.forward) * -Vector3.up;
            graph.DrawLine(marker * 0.9f, marker * 1.1f, c);
            marker = Quaternion.AngleAxis(360.0f * legStates[leg].strikeTime, -Vector3.forward) * -Vector3.up;
            graph.DrawLine(marker * 0.9f, marker * 1.1f, c);
        }
        GL.End();
        GL.Begin(GL.QUADS);
        for (int leg = 0; leg < legs.Length; leg++)
        {
            Color c = legs[leg].debugColor;
            Vector3 cycleVect;
            cycleVect = Quaternion.AngleAxis(360.0f * legStates[leg].cycleTime, -Vector3.forward) * -Vector3.up;
            graph.DrawRect(cycleVect - Vector3.one * 0.1f, cycleVect + Vector3.one * 0.1f, c);
            float dictatedCycle = Util.CyclicDiff(normalizedTime, legStates[leg].stanceTime);
            cycleVect = Quaternion.AngleAxis(360.0f * dictatedCycle, -Vector3.forward) * -Vector3.up * 0.8f;
            graph.DrawRect(cycleVect - Vector3.one * 0.05f, cycleVect + Vector3.one * 0.05f, c);
        }
        GL.End();
    }

    public void RenderMotionCycles()
    {
        float h = Camera.main.pixelHeight;
        Color weakColor = new Color(0.7f, 0.7f, 0.7f, 1);

        for (int m = 0; m < legC.cycleMotions.Length; m++)
        {
            DrawArea graph = new DrawArea(new Vector3(0.01f * h + 0.2f * h * m, 0.31f * h, 0), new Vector3(0.19f * h + 0.2f * h * m, 0.49f * h, 0));
            graph.canvasMin = new Vector3(-1.1f, -1.1f, 0);
            graph.canvasMax = new Vector3(1.1f, 1.1f, 0);

            GL.Begin(GL.LINES);
            graph.DrawLine(-0.9f * Vector3.up, -1.1f * Vector3.up, weakColor);
            for (int i = 0; i < 90; i++)
            {
                graph.DrawLine(Quaternion.AngleAxis(i / 90.0f * 360, Vector3.forward) * Vector3.up, Quaternion.AngleAxis((i + 1) / 90.0f * 360,
                                                    Vector3.forward) * Vector3.up, weakColor);
            }
            GL.End();
            GL.Begin(GL.QUADS);
            for (int leg = 0; leg < legs.Length; leg++)
            {
                Color c = legs[leg].debugColor;
                Vector3 cycleVect;
                float t = (legC.cycleMotions[m].cycles[leg].stanceTime - legC.cycleMotions[m].cycleOffset * (0.5f + 0.5f * Mathf.Sin(Time.time * 2)));
                cycleVect = Quaternion.AngleAxis(360.0f * t, -Vector3.forward) * -Vector3.up;
                graph.DrawRect(cycleVect - Vector3.one * 0.1f, cycleVect + Vector3.one * 0.1f, c);
            }
            GL.End();
        }
    }

    public void RenderFootMarkers()
    {
        GL.Begin(GL.LINES);

        GL.End();
        GL.Begin(GL.QUADS);
        Vector3 heel, forward, up, right;
        Matrix4x4 m;
        for (int leg = 0; leg < legs.Length; leg++)
        {
            for (int step = 0; step < 3; step++)
            {
                if (legStates[leg] == null) continue;
                if (step == 0)
                {
                    m = legStates[leg].stepFromMatrix;
                    GL.Color(legs[leg].debugColor * 0.8f);
                }
                else if (step == 1)
                {
                    m = legStates[leg].stepToMatrix;
                    GL.Color(legs[leg].debugColor);
                }
                else
                {
                    m = legStates[leg].stepToMatrix;
                    GL.Color(legs[leg].debugColor * 0.4f);
                }

                heel = m.MultiplyPoint3x4(Vector3.zero);
                forward = m.MultiplyVector(legStates[leg].heelToetipVector);
                up = m.MultiplyVector(Vector3.up);
                right = (Quaternion.AngleAxis(90, up) * forward).normalized * legs[leg].footWidth * scale;
                heel += up.normalized * right.magnitude / 20;
                if (step == 2) { heel += legStates[leg].stepToPositionGoal - legStates[leg].stepToPosition; }
                GL.Vertex(heel + right / 2);
                GL.Vertex(heel - right / 2);
                GL.Vertex(heel - right / 4 + forward);
                GL.Vertex(heel + right / 4 + forward);
            }
        }
        GL.End();
    }

    void OnGUI()
    {
        if (renderAnimationStates) RenderAnimationStates();
    }

    public void RenderAnimationStates()
    {
        int i = 0;
        foreach (AnimationState state in animation)
        {
            string str = state.name;
            float v = 0.5f + 0.5f * state.weight;
            GUI.color = new Color(0, 0, v, 1);
            if (state.enabled)
            {
                GUI.color = new Color(v, v, v, 1);
            }
            str += " " + state.weight.ToString("0.000");
            GUI.Label(new Rect(Screen.width - 200, 10 + 20 * i, 200, 20), str);
            i++;
        }
    }

}
